import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    # 获取功能包路径
    pkg_path = get_package_share_directory('zorbus_description')
    urdf_file = os.path.join(pkg_path, 'urdf', 'so101_new_calib.urdf')
    rviz_config = os.path.join(pkg_path, 'rviz', 'view.rviz')

    return LaunchDescription([

        # 启动关节状态发布器（GUI）
        Node(
            name='joint_state_publisher_gui',
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            output='screen',
        ),

        # 启动机器人状态发布器（加载URDF）
        Node(
            name='robot_state_publisher',
            package='robot_state_publisher',
            executable='robot_state_publisher',
            arguments=[urdf_file],
            output='both',
        ),

        # 启动RViz（加载预配置）
        Node(
            name='rviz2',
            package='rviz2',
            executable='rviz2',
            arguments=['-d', rviz_config],
            output='screen'
        )
    ])
